//
// Created by lqk on 2022/2/13.
//

#ifndef QUADCEREBELLUM_PYBULLETMOTORSDRIVER_H
#define QUADCEREBELLUM_PYBULLETMOTORSDRIVER_H

#include "PeriodicTaskManager.h"
#include "PeriodicTask.h"
#include "MotorsStructs.h"
#include "quad_bot/torques_pb.hpp"
#include "quad_bot/motors_info_pb.hpp"
#include <lcm/lcm-cpp.hpp>
#include <lcm/lcm.h>

extern MotorsInfo g_motorsInfo;
extern MotorsCommand g_motorsCommand;
extern int number;

class PyBulletMotorsDriver :public PeriodicTask {
public:
    PyBulletMotorsDriver(PeriodicTaskManager* taskManager,const float& period,const std::string& name,const std::string& pubChannel,const std::string& subChannel):PeriodicTask(taskManager,period,name),m_pubChannel(pubChannel),m_subChannel(subChannel){}
    virtual void Init() override;
    virtual void RunOnce() override;
    virtual void PrintStatus() override;
private:
    void SendCommand();
    virtual void CleanUp() override;
    void ActuatorSimulate();
    void Callback(const lcm::ReceiveBuffer* rbuf,const std::string& channel,const quad_bot::motors_info_pb* msg);
    void TimeOutPrinter(int);

    float motorsTorque[12]{};
    lcm::LCM m_lcmPub,m_lcmSub;
    const std::string m_pubChannel,m_subChannel;
    int m_timeOut=2000;
    int m_timeOutCounter=0;
    int m_timeOutFlag=0;
};


#endif //QUADCEREBELLUM_PYBULLETMOTORSDRIVER_H
